# -*- coding: utf-8 -*

import util
import distance

def nullHeuristic(state, problem=None):
  """
  A heuristic function estimates the cost from the current state to the nearest
  goal in the provided SearchProblem.  This heuristic is trivial.
  """
  return 0

def geoHeuristic(state, problem):
  geo1 = state.curStop.location
  geo2 = problem.goalSpot.location
  dist = distance.Distance().getDistance(geo1.lat, geo1.lon, geo2.lat, geo2.lon)
  return dist
  
def aStarSearch(problem, heuristic=nullHeuristic):
  "Search the node that has the lowest combined cost and heuristic first."
  "*** YOUR CODE HERE ***"

  # f() = g() + h()
  g = problem.getCostOfActions
  h = heuristic
  f = lambda actions, state: g(actions) + h(state, problem)
  
  frontier = util.PriorityQueue()
  visit = list() # state
  dest = dict() # actions => state
  maxSpace = 0
  
  startActions = tuple([])
  startState = problem.getStartState()
  frontier.push(startActions, f(startActions, startState))
  dest[startActions] = startState
  visit.append(startState)

  #cnt = 0
  while not frontier.isEmpty():
    #if cnt > 2:
    #  break
    if len(frontier.heap) + len(visit) + len(dest) * 2 > maxSpace:
      maxSpace = len(frontier.heap) + len(visit) + len(dest) * 2
    top = frontier.pop()
    #print "top:", dest[top]
    if problem.isGoalState(dest[top]):
      #print "maximum space required:", maxSpace
      return list(top)
    for succ in problem.getSuccessors(dest[top]):
      if succ[0] not in visit:
        #print "    succ:", succ[0]
        newActions = list(top)
        newActions.append(succ[1])
        newActions = tuple(newActions)
        frontier.push(newActions, f(newActions, succ[0]))
        dest[newActions] = succ[0]
        visit.append(dest[newActions])
    #cnt += 1
    
  return None
